% Running the reinforcement learning test. Starting with agent in map at given (pos)ition

function collectedReward = hw7Run(agent, map, pos)
%Setting variables and obtaining map information
litterMap = map(:,:,3);
[RMAX,CMAX,mWorlds] = size(map);
agentPosx = pos(1);
agentPosy = pos(2);
RMIN = 1;
CMIN  = 1;

collectedReward = 0;
litterQtable = agent.litter;
% Number of actions in every state. 5 - look E,N,N,NE,SE
%Let 1-N, 2-NE, 3-E, 4-SE, 5-S
NUM_ACTIONS = 5;

ilMap = litterMap;
agentLitter = zeros(11,2);
agentLitter(1,1) = agentPosx;
agentLitter(1,2) = agentPosy; 
k = 2;
for i=1:RMAX
    for j=1:CMAX
        if(litterMap(i,j) == 10)
            % found litter
            ilMap(i,j) = 120;
             agentLitter(k,1) = i;
             agentLitter(k,2) = j;
             k = k+1;
        end
    end
end

% calculate distances from the litters
allDistances = pdist(agentLitter)';

for i=1:10
    distancesFromLitter(i) = allDistances(i);
end
[agentLitterDistances, litterIndices] = sort(distancesFromLitter);

i=1;
allBehind = 1;
while i<=10
    litterPos = agentLitter(litterIndices(i)+1,:);
    if(litterPos(2) < agentPosy)
        i = i+1;
        continue
    else
        nearestLitterX = agentLitter(litterIndices(i)+1,1);
        nearestLitterY = agentLitter(litterIndices(i)+1,2);
        i=11;
        allBehind = 0;
    end
end
toContinue = 1;
allBehind
% incase all litters are behind me
if(allBehind == 1)
    toContinue = 0;
end

if (toContinue == 1)
    
    ySteps = nearestLitterY - int8(agentPosy);
    xSteps = nearestLitterX - int8(agentPosx);
    if (xSteps < 0 && ySteps == 0 ) % N
        direction = 1;
    elseif (xSteps < 0 && ySteps > 0 ) % NE
        direction = 2;
    elseif (xSteps == 0 && ySteps > 0 ) % E
        direction = 3;
    elseif (xSteps > 0 && ySteps > 0 ) % SE
        direction = 4;
    elseif (xSteps > 0 && ySteps ==0 ) % S
        direction = 5;
    end

end

findNewVal = 0;
image(ilMap);
hold on
pos1 = [agentPosx,agentPosy];
pos2 = [agentPosx,agentPosy];
newposi = zeros(1,2);


while (toContinue ~= 0)
    if (findNewVal)
        nextDir = uint8((NUM_ACTIONS-1)*rand(1)+1);
        findNewVal = 0;
    else
        [maxVal,nextDir] = max(litterQtable(ySteps+1,direction,:));
    end
    
    if(nextDir == 1) % North
        if(agentPosx == RMIN)
              findNewVal = 1;
				continue

        end
        agentPosx = agentPosx - 1;		
    elseif(nextDir == 2) % North East
        if(agentPosx == RMIN || agentPosy==CMAX)
              findNewVal = 1;
				continue
        end
        agentPosx = agentPosx - 1;		
        agentPosy = agentPosy + 1;		
    elseif(nextDir == 3) % East
        if(agentPosy == CMAX)
              findNewVal = 1;
				continue
        end
        agentPosy = agentPosy + 1;		
    elseif(nextDir == 4) % South East
        if(agentPosx == RMAX || agentPosy==CMAX)
              findNewVal = 1;
				continue
        end
        agentPosx = agentPosx + 1;		
        agentPosy = agentPosy + 1;		
    elseif(nextDir == 5) % South
        if(agentPosx == RMAX)
              findNewVal = 1;
				continue
        end
        agentPosx = agentPosx + 1;		
    end

    h = plot(agentPosy,agentPosx,'.');
    newposi = [agentPosx,agentPosy];
    if(newposi == pos2)
        findNewVal = 1;
        continue
    else
        pos2 = pos1;
        pos1 = newposi;
    end
    set(h,'MarkerSize',24)
    hold on

    %correction based on new information
    reward = litterMap(agentPosx,agentPosy);
    collectedReward = collectedReward + reward;
    newySteps = nearestLitterY - int8(agentPosy);
    newxSteps = nearestLitterX - int8(agentPosx);

    if (newxSteps < 0 && newySteps == 0 ) % N
        newdirection = 1;
    elseif (newxSteps < 0 && newySteps > 0 ) % NE
        newdirection = 2;
    elseif (newxSteps == 0 && newySteps > 0 ) % E
        newdirection = 3;
    elseif (newxSteps > 0 && newySteps > 0 ) % SE
        newdirection = 4;
    elseif (newxSteps > 0 && newySteps ==0 ) % S
        newdirection = 5;
    end

    lookupY = ySteps+1;
    lookupNewY = newySteps+1;
    if(lookupNewY <= 0)
        toContinue = 0;
        continue
    end
      %update dist's
    ySteps = newySteps;
    direction = newdirection;

    if (toContinue == -1)
        toContinue = 0;
    elseif (toContinue ~= 0 && agentPosx == nearestLitterX && agentPosy == nearestLitterY)
        toContinue = -1;
    end
end

